clear all; clc;

bagdata = rosbag('.\bagData\vilvo_2019-02-26-23-56-34.bag');

imgtopic = select(bagdata,'Topic','/camera/0/0/image/compressed');
birdtopic = select(bagdata,'Topic','/camera/image/birdview_seg/compressed');
% % vehicletopic = select(bagdata,'Topic','/deepps/vehicle_info');
freetopic = select(bagdata,'Topic','/freespace/freespace_image/compressed');
scantopic = select(bagdata,'Topic','/freespace/scan');
imutopic = select(bagdata,'Topic','/imu');

imgdata = readMessages(imgtopic);
% birddata = readMessages(birdtopic);
% vehicledata = readMessages(vehicletopic);
% scandata = readMessages(scantopic);
imudata = readMessages(imutopic);

% %img
% for i = 1:imgtopic.NumMessages
%     image = imgdata(i);
%     image_timestamp = vpa(image{1}.Header.Stamp.seconds*1000);
%     image_out = readImage(image{1});
%     name = sprintf("./saveBag/26-23-56-34/img/%f.png",image_timestamp);
%     imwrite(image_out, name); 
% end


%imu
imu_fid = fopen('./saveBag/26-23-56-34/imu.txt','w');
for i = 1:imutopic.NumMessages
    imu = imudata(i);
    imu_timestamp = vpa(imu{1}.Header.Stamp.seconds*1000);
    acc_x = imu{1}.LinearAcceleration.X;
    acc_y = imu{1}.LinearAcceleration.Y;
    acc_z = imu{1}.LinearAcceleration.Z; 
    
    vel_x = imu{1}.AngularVelocity.X;
    vel_y = imu{1}.AngularVelocity.Y;
    vel_z = imu{1}.AngularVelocity.Z;
    
    ori_x = imu{1}.Orientation.X;
    ori_y = imu{1}.Orientation.Y;
    ori_z = imu{1}.Orientation.Z;
    ori_w = imu{1}.Orientation.W;
    
    fprintf(imu_fid, '%f %f %f %f %f %f %f %f %f %f %f\n', imu_timestamp, acc_x,acc_y,acc_z,vel_x,vel_y,vel_z,ori_x,ori_y,ori_z,ori_w);
end
fclose(imu_fid);